#include "polygon.h"


Polygon::Polygon (const f64 *data, u64 size) noexcept {

  this->x = data[0];
  this->y = data[1];
  this->n = (size - 4) / 2;

  for (auto i = 2UL; i < size; i += 2)
    this->px.emplace_back(data[i] - this->x);

  for (auto i = 3UL; i < size; i += 2)
    this->py.emplace_back(data[i] - this->y);

  for (auto i = 0; i < this->n + 1; ++i)
    this->pdp.emplace_back(this->px[i] * this->px[i] + this->py[i] * this->py[i]);

  for (auto i = 0; i < this->n; ++i)
    this->tdp.emplace_back(this->px[i] * this->px[i+1] + this->py[i] * this->py[i+1]);

  for (auto i = 0; i < this->n; ++i)
    this->tf.emplace_back(1.0 / (this->pdp[i] * this->pdp[i+1] - this->tdp[i] * this->tdp[i]));

  for (auto i = 0; i < this->n; ++i) {
    auto cp = this->px[i] * this->py[i + 1] - this->py[i] * this->px[i + 1];
    if (cp > 0.0)
      this->tc.emplace_back(1L);
    else if (cp < 0.0)
      this->tc.emplace_back(-1L);
    else
      this->tc.emplace_back(0L);
  }
}


s64v4 Polygon::contains (f64v4 x, f64v4 y) noexcept {

  auto dx = x - _mm256_set1_pd(this->x);
  auto dy = y - _mm256_set1_pd(this->y);

  auto px = this->px.data();
  auto py = this->py.data();
  auto pdp = this->pdp.data();
  auto tdp = this->tdp.data();
  auto tc = this->tc.data();
  auto tf = this->tf.data();
  auto _0 = _mm256_set1_epi64x(0L);
  auto _1 = _mm256_set1_epi64x(1L);
  auto res = _0;

  for (auto i = 0, j = 1; i < this->n; ++i, ++j) {
    auto dp0 = dx * _mm256_set1_pd(px[i]) + dy * _mm256_set1_pd(py[i]);
    auto dp1 = dx * _mm256_set1_pd(px[j]) + dy * _mm256_set1_pd(py[j]);
    auto pdp0 = _mm256_set1_pd(pdp[i]);
    auto pdp1 = _mm256_set1_pd(pdp[j]);
    auto tdp_ = _mm256_set1_pd(tdp[i]);
    auto tf_ = _mm256_set1_pd(tf[i]);
    auto u = (pdp1 * dp0 - tdp_ * dp1) * tf_;
    auto v = (pdp0 * dp1 - tdp_ * dp0) * tf_;
    auto uv = u + v;
    auto uc = _mm256_cmp_pd(_0, u, _CMP_LE_OS);
    auto vc = _mm256_cmp_pd(_0, v, _CMP_LE_OS);
    auto uvc = _mm256_cmp_pd(_1, uv, _CMP_LE_OS);
    auto msk = _mm256_and_pd(uvc, _mm256_and_pd(uc, vc));
    auto cnt = (s64v4)_mm256_blendv_pd((f64v4)_0, (f64v4)_mm256_set1_epi64x(tc[i]), msk);
    res = res + cnt;
  }

  return avx_cast<s64v4>(res);
}